// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ObjectDetectionFeaturization.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Vision.ObjectDetection
{
    using System;
    using System.Collections.Generic;
    using System.Windows;
    using System.Windows.Media.Imaging;

    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Vision;
    using Microsoft.Search;

    /// <summary>
    /// Creates feature vectors for object detection
    /// </summary>
    public class ObjectDetectionFeaturization
    {
        /// <summary>
        /// Gets the associated IR frame
        /// </summary>
        public readonly ImageFrame<ushort> IrFrame;

        /// <summary>
        /// Gets the associated depth frame
        /// </summary>
        public readonly ImageFrameDepth DepthFrame;

        /// <summary>
        /// Scaling factor for normalizing pixel sums. The value is
        /// not critical just needs to be in the correct order of magnitude
        /// </summary>
        private const double SumScaleFactor = 10000.0;

        /// <summary>
        /// Scaling factor for scaling cluster distance features. The value is
        /// not critical just needs to be in the correct order of magnitude
        /// </summary>
        private const double ClusterDistanceScaleFactor = 1000.0;

        /// <summary>
        /// Integral IR frame
        /// </summary>
        private ImageFrame<double> integralIrFrame;

        /// <summary>
        /// Integral depth frame
        /// </summary>
        private ImageFrame<double> integralDepthFrame;

        /// <summary>
        /// Integral normal frame for positive angles
        /// </summary>
        private ImageFrame<Vector3> integralPosNormalFrame;

        /// <summary>
        /// Integral normal frame for negative angles
        /// </summary>
        private ImageFrame<Vector3> integralNegNormalFrame;

        /// <summary>
        /// Average IR image value
        /// </summary>
        private double avgIRvalue;

        /// <summary>
        /// Average depth image value
        /// </summary>
        private double avgDepthValue;

        /// <summary>
        /// Initializes a new instance of the <see cref="ObjectDetectionFeaturization"/> class.
        /// </summary>
        /// <param name="irImage">IR image</param>
        /// <param name="depthImage">Depth image</param>
        /// <param name="fx">Focal length x</param>
        /// <param name="fy">Focal Length y</param>
        public ObjectDetectionFeaturization(ImageFrame<ushort> irImage, ImageFrameDepth depthImage, double fx, double fy)
        {
            if (null == irImage || null == depthImage)
            {
                throw new ArgumentException("Input images cannot be null");
            }

            if (irImage.Width != depthImage.Width || irImage.Height != depthImage.Height)
            {
                throw new ArgumentException(string.Format("IR image ({0}, {1}) size != Depth image size ({2}, {3})", irImage.Width, irImage.Height, depthImage.Width, depthImage.Height));
            }

            this.IrFrame = irImage;
            this.DepthFrame = depthImage;
            this.NormalFrame = new ImageFrameNormal(this.DepthFrame, fx, fy);
            this.BuildIntegralframes();
        }

        /// <summary>
        /// Gets the associated normal frame
        /// </summary>
        public ImageFrameNormal NormalFrame { get; private set; }

        /// <summary>
        /// Compute post processor confidence estimator features from a detected patch
        /// </summary>
        /// <param name="cluster">Input patch cluster</param>
        /// <param name="samplingFactor">Down scale pixel sampling factor</param>
        /// <returns>Feature vector</returns>
        public static int[] GetPostProcessorFeatures(PatchCluster cluster, int samplingFactor)
        {
            List<int> feats = new List<int>();

            feats.Add((int)(cluster.MeanConfidence * ObjectDetectionFeaturization.SumScaleFactor));
            feats.Add((int)(cluster.StdDevConfidence * ObjectDetectionFeaturization.SumScaleFactor));
            feats.Add((int)(cluster.DistanceMean * ObjectDetectionFeaturization.ClusterDistanceScaleFactor));
            feats.Add((int)(cluster.DistanceStdDev * ObjectDetectionFeaturization.ClusterDistanceScaleFactor));
            feats.Add((int)(cluster.Components.Count * ObjectDetectionFeaturization.SumScaleFactor / (cluster.PatchHeight * cluster.PatchWidth) * samplingFactor));

            return feats.ToArray();
        }

        /// <summary>
        /// Get Patch classification features from the RGB image, given the center and size of patch
        /// </summary>
        /// <param name="inputRgbFrame">Input RGB image</param>
        /// <param name="col">Center column</param>
        /// <param name="row">Center row</param>
        /// <param name="widthPix">RGB patch width</param>
        /// <param name="heightPix">RGB patch height</param>
        /// <param name="outPatchSize">Scaled output patch size</param>
        /// <returns>Classification features</returns>
        public static int[] GetClassificationFeatures(ImageFrameRgb inputRgbFrame, int col, int row, int widthPix, int heightPix, Size outPatchSize)
        {
            int[] feats = null;

            if (col < (widthPix / 2) || col > inputRgbFrame.Width - (widthPix / 2) ||
                row < (heightPix / 2) || row > inputRgbFrame.Height - (heightPix / 2))
            {
                return null;
            }

            int left = col - widthPix / 2;
            int top = row - heightPix / 2;

            ImageFrameRgb rgbPatch = new ImageFrameRgb((int)outPatchSize.Width, (int)outPatchSize.Height);
            ObjectDetectionFeaturization.ExtractAndResizePatch(inputRgbFrame, rgbPatch, new Rect(left, top, widthPix, heightPix));

            int totLength = rgbPatch.Width * rgbPatch.Height * inputRgbFrame.BytesPerPixel;
            feats = new int[totLength];

            int featCount = 0;
            for (int i = 0; i < rgbPatch.Width; ++i)
            {
                for (int j = 0; j < rgbPatch.Height; ++j)
                {
                    feats[featCount++] = rgbPatch[i, j].Red;
                    feats[featCount++] = rgbPatch[i, j].Green;
                    feats[featCount++] = rgbPatch[i, j].Blue;
                }
            }

            return feats;
        }

        /// <summary>
        /// Extract image pixels from a image sub rectangle
        /// </summary>
        /// <param name="inputFrame">Input image frame></param>
        /// <param name="outputFrame">Output frame containing sub rectangle</param>
        /// <param name="extractRect">Requested size of rectangle</param>
        public static void ExtractAndResizePatch(ImageFrameRgb inputFrame, ImageFrameRgb outputFrame, Rect extractRect)
        {
            if (outputFrame.Width <= 0 || outputFrame.Height <= 0)
            {
                throw new ArgumentException(string.Format("output frame ({0} {1}) is invalid", outputFrame.Width, outputFrame.Height), "outputFrame");
            }

            double xScale = (double)extractRect.Width / (double)outputFrame.Width;
            double yScale = (double)extractRect.Height / (double)outputFrame.Height;
            double xScaleBy2 = xScale / 2.0;
            double yScaleBy2 = yScale / 2.0;

            for (int i = 0; i < outputFrame.Width; ++i)
            {
                for (int j = 0; j < outputFrame.Height; ++j)
                {
                    double xCenter = xScale * i + extractRect.Left;
                    double yCenter = yScale * j + extractRect.Top;

                    outputFrame[i, j] = ObjectDetectionFeaturization.DownSample(inputFrame, xCenter, yCenter, xScaleBy2, yScaleBy2);
                }
            }
        }

        /// <summary>
        /// Return set of features for a patch centered at pixel {column, row}. The feature
        /// set is sum of pixel values in the patch and the differences pixel sums in sub rectangles
        /// as in <see cref="http://www2.it.lut.fi/kurssit/03-04/010970000/lectures/papers/violaNIPS-01.pdf"/>.
        /// There are a total of 15 features:
        /// 5 x 2 = 10 - Sum and 4 difference filters for IR and depth frames.
        ///          3 - Sum of positive surface normal angles
        ///          2 - Sum of negative surface normal angles  (Don't need z component as it is always zero)
        /// </summary>
        /// <param name="col">Requested column</param>
        /// <param name="row">Requested row</param>
        /// <param name="patchWidthPix">Patch width in pixels used</param>
        /// <param name="patchHeightPix">Patch height in pixels used</param>
        /// <returns>Feature vector or null if requested patch cannot be centered at {col, row}</returns>
        public int[] GetIntegralFeatures(int col, int row, int patchWidthPix, int patchHeightPix)
        {
            short depth = this.DepthFrame[col, row];
            int heightBy2 = patchHeightPix / 2;
            int widthBy2 = patchWidthPix / 2;
            int[] feats = null;
            patchWidthPix = widthBy2 * 2;
            patchHeightPix = heightBy2 * 2;

            // Can requested patch size be created at {col, row}
            if (patchWidthPix > 0 &&
                patchHeightPix > 0 &&
                col >= (widthBy2 - 1) &&
                 col < this.NormalFrame.Width - widthBy2 &&
                 row >= (heightBy2 - 1) &&
                 row < this.NormalFrame.Height - heightBy2)
            {
                feats = new int[15];
                int idx = 0;
                int colLeft = col - widthBy2 + 1;
                int colRight = col + widthBy2;
                int rowTop = row - heightBy2 + 1;
                int rowBottom = row + heightBy2;
                double patchSize = widthBy2 * heightBy2 * 4;
                double normIR = patchSize * this.avgIRvalue / ObjectDetectionFeaturization.SumScaleFactor;
                double normDepth = patchSize * this.avgDepthValue / ObjectDetectionFeaturization.SumScaleFactor;

                feats[idx++] = (int)((this.GetSumInPatch(this.integralIrFrame, colLeft, rowTop, colRight, rowBottom) - patchSize * this.avgIRvalue) / normIR);
                feats[idx++] = (int)((this.GetSumInPatch(this.integralDepthFrame, colLeft, rowTop, colRight, rowBottom) - patchSize * this.avgDepthValue) / normDepth);

                feats[idx++] = (int)((this.GetSumInPatch(this.integralIrFrame, colLeft, rowTop, col, rowBottom) -
                                this.GetSumInPatch(this.integralIrFrame, col + 1, rowTop, colRight, row)) / normIR);
                feats[idx++] = (int)((this.GetSumInPatch(this.integralDepthFrame, colLeft, rowTop, col, rowBottom) -
                                this.GetSumInPatch(this.integralDepthFrame, col + 1, rowTop, colRight, row)) / normDepth);

                feats[idx++] = (int)((this.GetSumInPatch(this.integralIrFrame, colLeft, rowTop, colRight, row) -
                                this.GetSumInPatch(this.integralIrFrame, colLeft, row + 1, colRight, row)) / normIR);
                feats[idx++] = (int)((this.GetSumInPatch(this.integralDepthFrame, colLeft, rowTop, colRight, row) -
                                this.GetSumInPatch(this.integralDepthFrame, colLeft, row + 1, colRight, row)) / normDepth);

                int widthBy4 = widthBy2 / 2;
                int heightBy4 = heightBy2 / 2;
                feats[idx++] = (int)((this.GetSumInPatch(this.integralIrFrame, colLeft, rowTop, col - widthBy4, rowBottom) +
                                this.GetSumInPatch(this.integralIrFrame, col + widthBy4 + 1, rowTop, colRight, rowBottom) -
                                this.GetSumInPatch(this.integralIrFrame, col - widthBy4 + 1, rowTop, col + widthBy4, rowBottom)) / normIR);
                feats[idx++] = (int)((this.GetSumInPatch(this.integralDepthFrame, colLeft, rowTop, col - widthBy4, rowBottom) +
                                this.GetSumInPatch(this.integralDepthFrame, col + widthBy4 + 1, rowTop, colRight, rowBottom) -
                                this.GetSumInPatch(this.integralDepthFrame, col - widthBy4 + 1, rowTop, col + widthBy4, rowBottom)) / normDepth);

                feats[idx++] = (int)((this.GetSumInPatch(this.integralIrFrame, colLeft, rowTop, col, row) -
                                this.GetSumInPatch(this.integralIrFrame, col + 1, rowTop, colRight, row) -
                                this.GetSumInPatch(this.integralIrFrame, colLeft, row + 1, col, rowBottom) +
                                this.GetSumInPatch(this.integralIrFrame, col + 1, row + 1, colRight, rowBottom)) / normIR);
                feats[idx++] = (int)((this.GetSumInPatch(this.integralDepthFrame, colLeft, rowTop, col, row) -
                                this.GetSumInPatch(this.integralDepthFrame, col + 1, rowTop, colRight, row) -
                                this.GetSumInPatch(this.integralDepthFrame, colLeft, row + 1, col, rowBottom) +
                                this.GetSumInPatch(this.integralDepthFrame, col + 1, row + 1, colRight, rowBottom)) / normDepth);

                Vector3 f = this.GetSumInPatch(this.integralPosNormalFrame, colLeft, rowTop, colRight, rowBottom);
                idx = this.AddNormalVectorFeatures(feats, f, idx);
                f = this.GetSumInPatch(this.integralNegNormalFrame, colLeft, rowTop, colRight, rowBottom);
                feats[idx++] = (int)(f.X);
                feats[idx++] = (int)(f.Y);
            }

            return feats;
        }

        /// <summary>
        /// Get Level 2 features from a rectangular patch. The rectangle is scaled to fit
        /// level 2 patch width and height
        /// Feature length is (widthPix * heightPix) * 5  (where 5 = 1 (IR) + 1 (Depth) + 3 (Surface Norma)l)
        /// </summary>
        /// <param name="col">Center column</param>
        /// <param name="row">Center row</param>
        /// <param name="widthPix">Rectangle in image width</param>
        /// <param name="heightPix">Rectangle in image height</param>
        /// <param name="level2PatchSize">Fixed rectangular level 2 patch size</param>
        /// <param name="noReading">No reading value</param>
        /// <returns>Level 2 features</returns>
        public int[] GetPatchFeaturesLevel2(int col, int row, int widthPix, int heightPix, Size level2PatchSize, short noReading)
        {
            int[] feats = null;

            Size requestSize = new Size(widthPix, heightPix);

            if (col < (requestSize.Width / 2) || col > this.IrFrame.Width - (requestSize.Width / 2) ||
                row < (requestSize.Height / 2) || row > this.IrFrame.Height - (requestSize.Height / 2))
            {
                return null;
            }

            ImageFrame<ushort> ir = ObjectDetectionFeaturization.GetLevel2Patch(this.IrFrame, new Point(col, row), requestSize, level2PatchSize);
            ImageFrameDepth depth = ObjectDetectionFeaturization.GetLevel2Patch(this.DepthFrame, new Point(col, row), requestSize, level2PatchSize, noReading);
            ImageFrame<PixNormal> normal = ObjectDetectionFeaturization.GetLevel2Patch(this.NormalFrame, new Point(col, row), requestSize, level2PatchSize);

            if (null == ir || null == depth || null == normal)
            {
                return null;
            }

            int totLength = (int)(5 * level2PatchSize.Width * level2PatchSize.Height);
            feats = new int[totLength];

            int featCount = 0;
            for (int i = 0; i < level2PatchSize.Width; ++i)
            {
                for (int j = 0; j < level2PatchSize.Height; ++j)
                {
                    feats[featCount++] = ir[i, j];
                }
            }

            for (int i = 0; i < level2PatchSize.Width; ++i)
            {
                for (int j = 0; j < level2PatchSize.Height; ++j)
                {
                    feats[featCount++] = depth[i, j];
                }
            }

            for (int i = 0; i < level2PatchSize.Width; ++i)
            {
                for (int j = 0; j < level2PatchSize.Height; ++j)
                {
                    feats[featCount++] = normal[i, j].CosX;
                }
            }

            for (int i = 0; i < level2PatchSize.Width; ++i)
            {
                for (int j = 0; j < level2PatchSize.Height; ++j)
                {
                    feats[featCount++] = normal[i, j].CosY;
                }
            }

            for (int i = 0; i < level2PatchSize.Width; ++i)
            {
                for (int j = 0; j < level2PatchSize.Height; ++j)
                {
                    feats[featCount++] = normal[i, j].CosZ;
                }
            }

            return feats;
        }

        /// <summary>
        /// Get the features for orientation estimation from an image patch
        /// </summary>
        /// <param name="col">Center column</param>
        /// <param name="row">Center row</param>
        /// <param name="widthPix">Patch width in pixels</param>
        /// <param name="heightPix">Patch height in pixels</param>
        /// <param name="level2PatchSize">Fixed patch size for level 2 features</param>
        /// <param name="noReading">depth no reading value</param>
        /// <returns>Feature set for orientation estimation</returns>
        public int[] GetOrientationFeatures(
            int col,
            int row,
            int widthPix,
            int heightPix,
            Size level2PatchSize,
            short noReading)
        {
            const int FeatCountPerPixel = 4;
            int[] feats = null;

            int[] featsL1 = this.GetIntegralFeatures(col, row, widthPix, heightPix);

            if (null == featsL1)
            {
                return null;
            }

            Size requestSize = new Size(widthPix, heightPix);

            ImageFrameDepth depthPatch = ObjectDetectionFeaturization.GetLevel2Patch(this.DepthFrame, new Point(col, row), requestSize, level2PatchSize, noReading);
            int totLength = (int)(FeatCountPerPixel * (level2PatchSize.Width)) + featsL1.Length;
            feats = new int[totLength];

            double normDepth = depthPatch[depthPatch.Width / 2, depthPatch.Height / 2] / ObjectDetectionFeaturization.SumScaleFactor;
            int pixIdx = 0;

            for (int i = 0; i < depthPatch.Width; ++i)
            {
                double sum = 0.0;
                double sum2 = 0.0;
                double min = depthPatch[i, 0];
                double max = min;
                int pixCount = 0;

                for (int j = 0; j < depthPatch.Height; ++j)
                {
                    short pix = depthPatch[i, j];
                    if (pix != noReading)
                    {
                        double depthPix = pix;
                        sum += depthPix;
                        sum2 += depthPix * depthPix;
                        min = Math.Min(min, depthPix);
                        max = Math.Max(max, depthPix);
                        ++pixCount;
                    }
                }

                pixCount = Math.Max(1, pixCount);
                double mean = sum / pixCount;
                feats[pixIdx++] = (int)(mean / normDepth);
                double std = Math.Sqrt(sum2 / pixCount - (mean * mean));
                feats[pixIdx++] = (int)(std / normDepth);
                feats[pixIdx++] = (int)(max / normDepth);
                feats[pixIdx++] = (int)(min / normDepth);
            }

            featsL1.CopyTo(feats, pixIdx);

            return feats;
        }

        /// <summary>
        /// Top level for extracting a rescaled image frame for a sub rectangle within an image
        /// </summary>
        /// <param name="inImage">Input image</param>
        /// <param name="center">Center point for rectangle</param>
        /// <param name="rect">Targeted rectangle</param>
        /// <param name="outPatchSize">Desired patch size</param>
        /// <param name="noReading">Pixel value for no reading</param>
        /// <returns>The image frame for the rectangle</returns>
        private static ImageFrameDepth GetLevel2Patch(ImageFrameDepth inImage, Point center, Size rect, Size outPatchSize, short noReading)
        {
            int imageWidth = (int)rect.Width;
            int imageHeight = (int)rect.Height;
            int left = (int)center.X - imageWidth / 2;
            int top = (int)center.Y - imageHeight / 2;

            ImageFrameDepth resize = new ImageFrameDepth((int)outPatchSize.Width, (int)outPatchSize.Height);
            ObjectDetectionFeaturization.ExtractAndResizePatch(inImage, resize, new Rect(left, top, rect.Width, rect.Height), noReading);

            return resize;
        }

        /// <summary>
        /// Top level for extracting a rescaled image frame for a sub rectangle within an image
        /// </summary>
        /// <param name="inImage">Input image</param>
        /// <param name="center">Center point for rectangle</param>
        /// <param name="rect">Targeted rectangle</param>
        /// <param name="outPatchSize">Desired patch size</param>
        /// <returns>The image frame for the rectangle</returns>
        private static ImageFrame<ushort> GetLevel2Patch(ImageFrame<ushort> inImage, Point center, Size rect, Size outPatchSize)
        {
            int imageWidth = (int)rect.Width;
            int imageHeight = (int)rect.Height;
            int left = (int)center.X - imageWidth / 2;
            int top = (int)center.Y - imageHeight / 2;

            ImageFrame<ushort> resize = new ImageFrame<ushort>((int)outPatchSize.Width, (int)outPatchSize.Height);
            ObjectDetectionFeaturization.ExtractAndResizePatch(inImage, resize, new Rect(left, top, rect.Width, rect.Height));

            return resize;
        }

        /// <summary>
        /// Top level for extracting a rescaled image frame for a sub rectangle within a surface normal image
        /// </summary>
        /// <param name="inImage">Input image</param>
        /// <param name="center">Center point for rectangle</param>
        /// <param name="imageSize">Targeted rectangle</param>
        /// <param name="outSize">Desired patch size</param>
        /// <returns>The image frame for the rectangle</returns>
        private static ImageFrame<PixNormal> GetLevel2Patch(ImageFrame<PixNormal> inImage, Point center, Size imageSize, Size outSize)
        {
            int imageWidth = (int)imageSize.Width;
            int imageHeight = (int)imageSize.Height;
            int left = (int)center.X - imageWidth / 2;
            int top = (int)center.Y - imageHeight;

            ImageFrame<PixNormal> resize = new ImageFrame<PixNormal>((int)outSize.Width, (int)outSize.Height);
            ExtractAndResizePatch(inImage, resize, new Rect(left, top, outSize.Width, outSize.Height));

            return resize;
        }

        /// <summary>
        /// Extract image pixels from a image sub rectangle
        /// </summary>
        /// <param name="inputFrame">Input image frame></param>
        /// <param name="outputFrame">Output frame containing sub rectangle</param>
        /// <param name="extractRect">Requested size of rectangle</param>
        private static void ExtractAndResizePatch(ImageFrame<ushort> inputFrame, ImageFrame<ushort> outputFrame, Rect extractRect)
        {
            if (outputFrame.Width <= 0 || outputFrame.Height <= 0)
            {
                throw new ArgumentException(string.Format("output frame ({0} {1}) is invalid", outputFrame.Width, outputFrame.Height), "outputFrame");
            }

            double xScale = (double)extractRect.Width / (double)outputFrame.Width;
            double yScale = (double)extractRect.Height / (double)outputFrame.Height;

            for (int i = 0; i < outputFrame.Width; ++i)
            {
                for (int j = 0; j < outputFrame.Height; ++j)
                {
                    double col = xScale * i + extractRect.Left;
                    double row = yScale * j + extractRect.Top;

                    outputFrame[i, j] = ObjectDetectionFeaturization.BilinearTxfm(inputFrame, col, row);
                }
            }
        }

        /// <summary>
        /// Extract image pixels from a image sub rectangle
        /// </summary>
        /// <param name="inputFrame">Input image frame></param>
        /// <param name="outputFrame">Output frame containing sub rectangle</param>
        /// <param name="extractRect">Requested size of rectangle</param>
        /// <param name="noReading">Pixel value for no reading</param>
        private static void ExtractAndResizePatch(ImageFrameDepth inputFrame, ImageFrameDepth outputFrame, Rect extractRect, short noReading)
        {
            if (outputFrame.Width <= 0 || outputFrame.Height <= 0)
            {
                throw new ArgumentException(string.Format("output frame ({0} {1}) is invalid", outputFrame.Width, outputFrame.Height), "outputFrame");
            }

            double xScale = (double)extractRect.Width / (double)outputFrame.Width;
            double yScale = (double)extractRect.Height / (double)outputFrame.Height;

            for (int i = 0; i < outputFrame.Width; ++i)
            {
                for (int j = 0; j < outputFrame.Height; ++j)
                {
                    double col = xScale * i + extractRect.Left;
                    double row = yScale * j + extractRect.Top;

                    outputFrame[i, j] = ObjectDetectionFeaturization.BilinearTxfm(inputFrame, col, row, noReading);
                }
            }
        }

        /// <summary>
        /// Extract image pixels from a surface normal image sub rectangle
        /// </summary>
        /// <param name="inputFrame">Input image frame></param>
        /// <param name="outputFrame">Output frame containing sub rectangle</param>
        /// <param name="extractRect">Requested size of rectangle</param>
        /// <returns>True if rectangle was extracted, false if the requested rectangle will not fit</returns>
        private static bool ExtractAndResizePatch(ImageFrame<PixNormal> inputFrame, ImageFrame<PixNormal> outputFrame, Rect extractRect)
        {
            if (extractRect.X < 0 || extractRect.Y < 0 || extractRect.X >= inputFrame.Width - extractRect.Width || extractRect.Y >= inputFrame.Height - extractRect.Height)
            {
                return false;
            }

            if (outputFrame.Width <= 0 || outputFrame.Height <= 0)
            {
                throw new ArgumentException(string.Format("output frame ({0} {1}) is invalid", outputFrame.Width, outputFrame.Height), "outputFrame");
            }

            double xScale = (double)extractRect.Width / (double)outputFrame.Width;
            double yScale = (double)extractRect.Height / (double)outputFrame.Height;

            for (int i = 0; i < outputFrame.Width; ++i)
            {
                for (int j = 0; j < outputFrame.Height; ++j)
                {
                    double col = xScale * i + extractRect.Left;
                    double row = yScale * j + extractRect.Top;

                    outputFrame[i, j] = BilinearTxfm(inputFrame, col, row);
                }
            }

            return true;
        }

        /// <summary>
        /// Extract sampled pixel value centered at a requested point by down sampling the 
        /// neighborhood with requested scale factors
        /// </summary>
        /// <param name="inputImage">Input image</param>
        /// <param name="xCenter">Center column position</param>
        /// <param name="yCenter">Center row position</param>
        /// <param name="xScaleBy2">Half x down scale factor</param>
        /// /// <param name="yScaleBy2">Half y down scale factor</param>
        /// <returns>Extracted pixel value</returns>
        private static RgbPix DownSample(ImageFrameRgb inputImage, double xCenter, double yCenter, double xScaleBy2, double yScaleBy2)
        {
            double left = Math.Max(0, xCenter - xScaleBy2);
            double top = Math.Max(0, yCenter - yScaleBy2);

            int leftIdx = (int)Math.Ceiling(left);
            int topIdx = (int)Math.Ceiling(top);

            double right = Math.Min(inputImage.Width - 1, xCenter + xScaleBy2);
            double bottom = Math.Min(inputImage.Height - 1, yCenter + yScaleBy2);

            int rightIdx = (int)Math.Floor(right);
            int bottomIdx = (int)Math.Floor(bottom);

            int redSum = 0;
            int blueSum = 0;
            int greenSum = 0;

            for (int x = leftIdx; x < rightIdx; ++x)
            {
                for (int y = topIdx; y < bottomIdx; ++y)
                {
                    redSum += inputImage[x, y].Red;
                    greenSum += inputImage[x, y].Green;
                    blueSum += inputImage[x, y].Blue;
                }
            }

            double topFrac = topIdx - top;
            double bottomFrac = bottom - bottomIdx;
            int yTop = (int)top;
            int yBottom = (int)Math.Ceiling(bottom);

            for (int x = leftIdx; x < rightIdx; ++x)
            {
                redSum += (int)(inputImage[x, yTop].Red * topFrac + inputImage[x, yBottom].Red * bottomFrac);
                greenSum += (int)(inputImage[x, yTop].Green * topFrac + inputImage[x, yBottom].Green * bottomFrac);
                blueSum += (int)(inputImage[x, yTop].Blue * topFrac + inputImage[x, yBottom].Blue * bottomFrac);
            }

            double leftFrac = leftIdx - left;
            double rightFrac = right - rightIdx;
            int xLeft = (int)left;
            int xRight = (int)Math.Ceiling(right);

            for (int y = topIdx; y < bottomIdx; ++y)
            {
                redSum += (int)(inputImage[xLeft, y].Red * leftFrac + inputImage[xRight, y].Red * rightFrac);
                greenSum += (int)(inputImage[xLeft, y].Green * leftFrac + inputImage[xRight, y].Green * rightFrac);
                blueSum += (int)(inputImage[xLeft, y].Blue * leftFrac + inputImage[xRight, y].Blue * rightFrac);
            }

            double scale = xScaleBy2 * yScaleBy2 * 4.0;
            RgbPix rgb = new RgbPix((byte)(redSum / scale), (byte)(greenSum / scale), (byte)(blueSum / scale));
            return rgb;
        }

        /// <summary>
        /// Extract pixel value at a real valued {row column} position using a bi linear transform
        /// of neighborhood pixels
        /// </summary>
        /// <param name="inputImage">Input image</param>
        /// <param name="x">Column position</param>
        /// <param name="y">Row position</param>
        /// <returns>Transformed value</returns>
        private static ushort BilinearTxfm(ImageFrame<ushort> inputImage, double x, double y)
        {
            int xFloorIdx = (int)Math.Floor(x);
            int xCeilIdx = (int)Math.Ceiling(x);
            double xFrac = x - xFloorIdx;

            int yFloorIdx = (int)Math.Floor(y);
            int yCeilIdx = (int)Math.Ceiling(y);
            double yFrac = y - yFloorIdx;

            int xNextIdx = xFloorIdx;
            if (xFloorIdx != xCeilIdx && xCeilIdx < inputImage.Width)
            {
                ++xNextIdx;
            }

            int yNextIdx = yFloorIdx;
            if (yFloorIdx != yCeilIdx && yCeilIdx < inputImage.Height)
            {
                ++yNextIdx;
            }

            double valUpRow = (1.0 - xFrac) * inputImage[xFloorIdx, yFloorIdx] + xFrac * inputImage[xNextIdx, yFloorIdx];
            double valDownRow = (1.0 - xFrac) * inputImage[xFloorIdx, yNextIdx] + xFrac * inputImage[xNextIdx, yNextIdx];

            double val = (1.0 - yFrac) * valUpRow + yFrac * valDownRow;
            return (ushort)val;
        }

        /// <summary>
        /// Extract pixel value at a real valued {row column} position using a bi linear transform
        /// of neighborhood pixels
        /// </summary>
        /// <param name="inputImage">Input image</param>
        /// <param name="x">Column position</param>
        /// <param name="y">Row position</param>
        /// <param name="noReading">Pixel value for no reading</param>
        /// <returns>Transformed value</returns>
        private static short BilinearTxfm(ImageFrame<short> inputImage, double x, double y, short noReading)
        {
            int xFloorIdx = (int)Math.Floor(x);
            int xCeilIdx = (int)Math.Ceiling(x);
            double xFrac = x - xFloorIdx;

            int yFloorIdx = (int)Math.Floor(y);
            int yCeilIdx = (int)Math.Ceiling(y);
            double yFrac = y - yFloorIdx;

            int xNextIdx = xFloorIdx;
            if (xFloorIdx != xCeilIdx && xCeilIdx < inputImage.Width)
            {
                ++xNextIdx;
            }

            int yNextIdx = yFloorIdx;
            if (yFloorIdx != yCeilIdx && yCeilIdx < inputImage.Height)
            {
                ++yNextIdx;
            }

            short pix = inputImage[xFloorIdx, yFloorIdx];
            short pixNext = inputImage[xNextIdx, yFloorIdx];
            double valUpRow = LinearCombineXPix(inputImage, xFloorIdx, xNextIdx, yFloorIdx, xFrac, noReading);
            double valDownRow = LinearCombineXPix(inputImage, xFloorIdx, xNextIdx, yNextIdx, xFrac, noReading);

            double val = (1.0 - yFrac) * valUpRow + yFrac * valDownRow;
            return (short)val;
        }

        /// <summary>
        /// Linearly combine adjacent depth image pixels. 
        /// When one pixels has a no reading value then skip linear combination and just
        /// choose closets pixel value
        /// </summary>
        /// <param name="inputImage">Input image</param>
        /// <param name="xFloorIdx">Left x index</param>
        /// <param name="xNextIdx">Right x Index</param>
        /// <param name="yFloorIdx">Pixel y index</param>
        /// <param name="xFrac">Linear combination factor</param>
        /// <param name="noReading">No reading value</param>
        /// <returns>Combined pixel value</returns>
        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        private static double LinearCombineXPix(ImageFrame<short> inputImage, int xFloorIdx, int xNextIdx, int yFloorIdx, double xFrac, short noReading)
        {
            short pix = inputImage[xFloorIdx, yFloorIdx];
            short pixNext = inputImage[xNextIdx, yFloorIdx];
            double val;

            if (pix != noReading && pixNext != noReading)
            {
                val = (1.0 - xFrac) * pix + xFrac * pixNext;
            }
            else
            {
                val = xFrac > 0.5 ? pixNext : pix;
            }

            return val;
        }

        /// <summary>
        /// Extract pixel value at a real valued {row column} position using a bi linear transform
        /// of neighborhood pixels for a surface normal image
        /// </summary>
        /// <param name="inputImage">Input image</param>
        /// <param name="x">Column position</param>
        /// <param name="y">Row position</param>
        /// <returns>Transformed value</returns>
        private static PixNormal BilinearTxfm(ImageFrame<PixNormal> inputImage, double x, double y)
        {
            int xFloorIdx = (int)Math.Floor(x);
            int xCeilIdx = (int)Math.Ceiling(x);
            double xFrac = x - xFloorIdx;

            int yFloorIdx = (int)Math.Floor(y);
            int yCeilIdx = (int)Math.Ceiling(y);
            double yFrac = y - yFloorIdx;

            int xNextIdx = xFloorIdx;
            if (xFloorIdx != xCeilIdx && xCeilIdx < inputImage.Width)
            {
                ++xNextIdx;
            }

            int yNextIdx = yFloorIdx;
            if (yFloorIdx != yCeilIdx && yCeilIdx < inputImage.Height)
            {
                ++yNextIdx;
            }

            PixNormal valUpRow = ((1.0 - xFrac) * inputImage[xFloorIdx, yFloorIdx]) + (xFrac * inputImage[xNextIdx, yFloorIdx]);
            PixNormal valDownRow = ((1.0 - xFrac) * inputImage[xFloorIdx, yNextIdx]) + (xFrac * inputImage[xNextIdx, yNextIdx]);

            PixNormal val = ((1.0 - yFrac) * valUpRow) + (yFrac * valDownRow);
            return val;
        }

        /// <summary>
        /// Get the sum of pixels in a rectangular patch using an integral image
        /// </summary>
        /// <param name="integralFrame">Input image</param>
        /// <param name="colLeft">Left edge of rectangle</param>
        /// <param name="rowTop">Top of rectangle</param>
        /// <param name="colRight">Right column of rectangle</param>
        /// <param name="rowBottom">Bottom of rectangle</param>
        /// <returns>Pixel sum in rectangle</returns>
        private double GetSumInPatch(ImageFrame<double> integralFrame, int colLeft, int rowTop, int colRight, int rowBottom)
        {
            double sum = integralFrame[colRight, rowBottom] - integralFrame[colRight, rowTop] - integralFrame[colLeft, rowBottom] + integralFrame[colLeft, rowTop];
            return sum;
        }

        /// <summary>
        /// Get the sum of pixels in a rectangular patch using an integral image for surface normal
        /// </summary>
        /// <param name="integralFrame">Input image</param>
        /// <param name="colLeft">Left edge of rectangle</param>
        /// <param name="rowTop">Top of rectangle</param>
        /// <param name="colRight">Right column of rectangle</param>
        /// <param name="rowBottom">Bottom of rectangle</param>
        /// <returns>Pixel sum in rectangle for each normal direction</returns>
        private Vector3 GetSumInPatch(ImageFrame<Vector3> integralFrame, int colLeft, int rowTop, int colRight, int rowBottom)
        {
            Vector3 sum = new Vector3(
                    integralFrame[colRight, rowBottom].X - integralFrame[colRight, rowTop].X - integralFrame[colLeft, rowBottom].X + integralFrame[colLeft, rowTop].X,
                    integralFrame[colRight, rowBottom].Y - integralFrame[colRight, rowTop].Y - integralFrame[colLeft, rowBottom].Y + integralFrame[colLeft, rowTop].Y,
                    integralFrame[colRight, rowBottom].Z - integralFrame[colRight, rowTop].Z - integralFrame[colLeft, rowBottom].Z + integralFrame[colLeft, rowTop].Z);

            return sum;
        }

        /// <summary>
        /// Add surface normal components to the feature vector
        /// </summary>
        /// <param name="feats">Feature vector where components are added</param>
        /// <param name="normal">Normal components to add</param>
        /// <param name="offset">starting offset in feature vector</param>
        /// <returns>next index in feature vector</returns>
        private int AddNormalVectorFeatures(int[] feats, Vector3 normal, int offset)
        {
            feats[offset++] = (int)(normal.X);
            feats[offset++] = (int)(normal.Y);
            feats[offset++] = (int)(normal.Z);
            return offset;
        }

        /// <summary>
        /// Builds all internal integral images from raw images
        /// </summary>
        private void BuildIntegralframes()
        {
            this.integralIrFrame = this.BuildIntegralFrame(this.IrFrame, out this.avgIRvalue);
            this.integralDepthFrame = this.BuildIntegralFrame(this.DepthFrame, out this.avgDepthValue);
            this.integralPosNormalFrame = this.BuildPositiveIntegralFrame(this.NormalFrame);
            this.integralNegNormalFrame = this.BuildNegativeIntegralFrame(this.NormalFrame);
        }

        /// <summary>
        /// Build an integral frame from an image frame
        /// </summary>
        /// <param name="inFrame">input frame</param>
        /// <param name="avg">Average pixel value</param>
        /// <returns>Integral frame</returns>
        private ImageFrame<double> BuildIntegralFrame(ImageFrameDepth inFrame, out double avg)
        {
            avg = 0.0;

            if (inFrame.Width > 0 && inFrame.Height > 0)
            {
                ImageFrame<double> integralFrame = new ImageFrame<double>(inFrame.Width, inFrame.Height);
                integralFrame[0, 0] = inFrame[0, 0];

                for (int i = 1; i < inFrame.Width; ++i)
                {
                    integralFrame[i, 0] = inFrame[i, 0] + integralFrame[i - 1, 0];
                    avg += inFrame[i, 0];
                }

                for (int j = 1; j < inFrame.Height; ++j)
                {
                    integralFrame[0, j] = inFrame[0, j] + integralFrame[0, j - 1];
                    avg += inFrame[0, j];
                }

                for (int i = 1; i < inFrame.Width; ++i)
                {
                    for (int j = 1; j < inFrame.Height; ++j)
                    {
                        integralFrame[i, j] = inFrame[i, j] + integralFrame[i - 1, j] + integralFrame[i, j - 1] - integralFrame[i - 1, j - 1];
                        avg += inFrame[i, j];
                    }
                }

                avg /= (inFrame.Width * inFrame.Height);
                return integralFrame;
            }

            return null;
        }

        /// <summary>
        /// Build an integral frame from an image frame
        /// </summary>
        /// <param name="inFrame">input frame</param>
        /// <param name="avg">Average pixel value</param>
        /// <returns>Integral frame</returns>
        private ImageFrame<double> BuildIntegralFrame(ImageFrame<ushort> inFrame, out double avg)
        {
            avg = 0.0;

            if (inFrame.Width > 0 && inFrame.Height > 0)
            {
                ImageFrame<double> integralFrame = new ImageFrame<double>(inFrame.Width, inFrame.Height);
                integralFrame[0, 0] = inFrame[0, 0];

                for (int i = 1; i < inFrame.Width; ++i)
                {
                    integralFrame[i, 0] = inFrame[i, 0] + integralFrame[i - 1, 0];
                    avg += inFrame[i, 0];
                }

                for (int j = 1; j < inFrame.Height; ++j)
                {
                    integralFrame[0, j] = inFrame[0, j] + integralFrame[0, j - 1];
                    avg += inFrame[0, j];
                }

                for (int i = 1; i < inFrame.Width; ++i)
                {
                    for (int j = 1; j < inFrame.Height; ++j)
                    {
                        integralFrame[i, j] = inFrame[i, j] + integralFrame[i - 1, j] + integralFrame[i, j - 1] - integralFrame[i - 1, j - 1];
                        avg += inFrame[i, j];
                    }
                }

                avg /= (inFrame.Width * inFrame.Height);
                return integralFrame;
            }

            return null;
        }

        /// <summary>
        /// Build an integral frame for normal image frames Use only positive angles
        /// </summary>
        /// <param name="inFrame">input frame</param>
        /// <returns>Built integral image</returns>
        private ImageFrame<Vector3> BuildPositiveIntegralFrame(ImageFrameNormal inFrame)
        {
            if (inFrame.Width <= 0 || inFrame.Height <= 0)
            {
                return null;
            }

            ImageFrame<Vector3> integralFrame = new ImageFrame<Vector3>(inFrame.Width, inFrame.Height);
            integralFrame[0, 0] = new Vector3(inFrame[0, 0].CosX, inFrame[0, 0].CosY, inFrame[0, 0].CosZ);

            for (int i = 1; i < inFrame.Width; ++i)
            {
                integralFrame[i, 0] = new Vector3(
                                Math.Max(inFrame[i, 0].CosX, (sbyte)0) + integralFrame[i - 1, 0].X,
                                 Math.Max(inFrame[i, 0].CosY, (sbyte)0) + integralFrame[i - 1, 0].Y,
                                Math.Max(inFrame[i, 0].CosZ, (sbyte)0) + integralFrame[i - 1, 0].Z);
            }

            for (int j = 1; j < inFrame.Height; ++j)
            {
                integralFrame[0, j] = new Vector3(
                                Math.Max(inFrame[0, j].CosX, (sbyte)0) + integralFrame[0, j - 1].X,
                                Math.Max(inFrame[0, j].CosY, (sbyte)0) + integralFrame[0, j - 1].Y,
                                Math.Max(inFrame[0, j].CosZ, (sbyte)0) + integralFrame[0, j - 1].Z);
            }

            for (int i = 1; i < inFrame.Width; ++i)
            {
                for (int j = 1; j < inFrame.Height; ++j)
                {
                    Vector3 pix = new Vector3(
                                        (Math.Max(inFrame[i, j].CosX, (sbyte)0) + integralFrame[i - 1, j].X + integralFrame[i, j - 1].X - integralFrame[i - 1, j - 1].X),
                                        (Math.Max(inFrame[i, j].CosY, (sbyte)0) + integralFrame[i - 1, j].Y + integralFrame[i, j - 1].Y - integralFrame[i - 1, j - 1].Y),
                                        (Math.Max(inFrame[i, j].CosZ, (sbyte)0) + integralFrame[i - 1, j].Z + integralFrame[i, j - 1].Z - integralFrame[i - 1, j - 1].Z));
                    integralFrame[i, j] = pix;
                }
            }

            return integralFrame;
        }

        /// <summary>
        /// Build an integral frame for normal image frames Use only negative angles
        /// </summary>
        /// <param name="inFrame">input frame</param>
        /// <returns>the built integral frame</returns>
        private ImageFrame<Vector3> BuildNegativeIntegralFrame(ImageFrameNormal inFrame)
        {
            if (inFrame.Width <= 0 || inFrame.Height <= 0)
            {
                return null;
            }

            ImageFrame<Vector3> integralFrame = new ImageFrame<Vector3>(inFrame.Width, inFrame.Height);
            integralFrame[0, 0] = new Vector3(inFrame[0, 0].CosX, inFrame[0, 0].CosY, inFrame[0, 0].CosZ);

            for (int i = 1; i < inFrame.Width; ++i)
            {
                integralFrame[i, 0] = new Vector3(
                                Math.Min(inFrame[i, 0].CosX, (sbyte)0) + integralFrame[i - 1, 0].X,
                                 Math.Min(inFrame[i, 0].CosY, (sbyte)0) + integralFrame[i - 1, 0].Y,
                                Math.Min(inFrame[i, 0].CosZ, (sbyte)0) + integralFrame[i - 1, 0].Z);
            }

            for (int j = 1; j < inFrame.Height; ++j)
            {
                integralFrame[0, j] = new Vector3(
                                Math.Min(inFrame[0, j].CosX, (sbyte)0) + integralFrame[0, j - 1].X,
                                Math.Min(inFrame[0, j].CosY, (sbyte)0) + integralFrame[0, j - 1].Y,
                                Math.Min(inFrame[0, j].CosZ, (sbyte)0) + integralFrame[0, j - 1].Z);
            }

            for (int i = 1; i < inFrame.Width; ++i)
            {
                for (int j = 1; j < inFrame.Height; ++j)
                {
                    Vector3 pix = new Vector3(
                                        (Math.Min(inFrame[i, j].CosX, (sbyte)0) + integralFrame[i - 1, j].X + integralFrame[i, j - 1].X - integralFrame[i - 1, j - 1].X),
                                        (Math.Min(inFrame[i, j].CosY, (sbyte)0) + integralFrame[i - 1, j].Y + integralFrame[i, j - 1].Y - integralFrame[i - 1, j - 1].Y),
                                        (Math.Min(inFrame[i, j].CosZ, (sbyte)0) + integralFrame[i - 1, j].Z + integralFrame[i, j - 1].Z - integralFrame[i - 1, j - 1].Z));
                    integralFrame[i, j] = pix;
                }
            }

            return integralFrame;
        }
    }
}
